from .servo import Servo
from .config import config
import time
import platform
if platform.machine()=="aarch64":
    import RPi.GPIO as GPIO
    GPIO.setmode(GPIO.BCM)
class handServo():
    r1,r2,r3,r4,r5=[],[],[],[],[]
    l1,l2,l3,l4,l5=[],[],[],[],[]
    pwm1,pwm2,pwm3,pwm4,pwm5= None,None,None,None,None
    status1,status2,status3,status4,status5=False,False,False,False,False

    def __init__(self):
        if platform.machine()=="aarch64":
            GPIO.setup(config['humb']["pin"], GPIO.OUT)
            self.pwm1 = GPIO.PWM(config['humb']["pin"],50)
            self.pwm1.start(config['humb']["min"])

            GPIO.setup(config['index']["pin"], GPIO.OUT)
            self.pwm2 = GPIO.PWM(config['index']["pin"],50)
 
            GPIO.setup(config['middle']["pin"], GPIO.OUT)
            self.pwm3 = GPIO.PWM(config['middle']["pin"],50)

            GPIO.setup(config['finger']["pin"], GPIO.OUT)
            self.pwm4 = GPIO.PWM(config['finger']["pin"],50)

            GPIO.setup(config['pinky']["pin"], GPIO.OUT)
            self.pwm5 = GPIO.PWM(config['pinky']["pin"],50)
            time.sleep(1)
        self.hand={
            "humb":Servo(config["humb"]),
            "index":Servo(config["index"]),
            "middle":Servo(config["middle"]),
            "finger":Servo(config["finger"]),
            "pinky":Servo(config["pinky"]),
        }
        self.start_time = time.time()
        self.current_nums = 0
        self.max_nums = 10
        pass
    def dirver(self,handlist):
        lefthand=handlist["left"]
        righthand=handlist["right"]
        if righthand:
            r1,r2,r3,r4,r5=zip(righthand)
            deg1, deg2, deg3, deg4, deg5 = self.hand["humb"].append(r1), self.hand["index"].append(r2), self.hand["middle"].append(r3), self.hand["finger"].append(r4), self.hand["pinky"].append(r5)
            if deg1>-1 and self.pwm1!=None: self.pwm1.start(deg1)
            if deg2>-1 and self.pwm2!=None: self.pwm2.start(deg2)
            if deg3>-1 and self.pwm3!=None: self.pwm3.start(deg3)
            if deg4>-1 and self.pwm4!=None: self.pwm4.start(deg4)
            if deg5>-1 and self.pwm5!=None: self.pwm5.start(deg5)
            if (deg1>-1 or deg2>-1 or deg3>-1 or deg4>-1 or deg5>-1) and (self.pwm1!=None or self.pwm2!=None or self.pwm3!=None or self.pwm4!=None or self.pwm5!=None):
                print("=======================b========================")
                print(deg1)
                print(deg2)
                print(deg3)
                print(deg4)
                print(deg5)
                print("=======================e========================")
                time.sleep(0.02)
                if deg1>-1: self.pwm1.stop()
                if deg2>-1: self.pwm2.stop()
                if deg3>-1: self.pwm3.stop()
                if deg4>-1: self.pwm4.stop()
                if deg5>-1: self.pwm5.stop()
